//
// Created by bourne on 2024/6/19.
//

#include <thread>
#include <iostream>
#include <fstream>

#include <lkh_tsp_solver/SolveTSP.h>
#include <lkh_mtsp_solver/SolveMTSP.h>

#include "exploration_manager/fast_exploration_manager.h"
#include "active_perception/graph_node.h"
#include "active_perception/graph_search.h"
#include "active_perception/frontier_finder.h"
#include "active_perception/hgrid.h"
#include "plan_env/raycast.h"
#include "plan_env/sdf_map.h"
#include "plan_manage/planner_manager.h"
#include "exploration_manager/expl_data.h"

using namespace Eigen;

namespace fast_planner {

FastExplorationManager::FastExplorationManager() {}

FastExplorationManager::~FastExplorationManager() {
  ViewNode::astar_.reset();
  ViewNode::caster_.reset();
  ViewNode::map_.reset();
}

/** initialize 函数是 FastExplorationManager 类的一个成员函数，
 * 用于初始化探索管理器的各种组件和参数。
 *
 * 初始化规划器管理器和相关的规划模块。
 * 创建和配置前沿发现器、层次化网格管理器以及探索数据和参数。
 * 从ROS参数服务器读取探索参数，包括局部细化、视点选择、TSP和ACVRP求解器的目录等。
 * 初始化无人机群体的状态数组和时间戳数组。
 * 初始化Astar搜索对象、射线投射器，并设置路径搜索的参数。
 * 创建TSP和ACVRP求解器的服务客户端。
 * 初始化无人机群体的状态参数，设置最后分配的网格ID和重新分配标志。
 * **/

void FastExplorationManager::initialize(ros::NodeHandle &nh) {
  // 创建并初始化快速规划器管理器
  planner_manager_.reset(new FastPlannerManager);
  planner_manager_->initPlanModules(nh);

  // 获取 EDT 环境和 SDF 地图对象
  edt_environment_ = planner_manager_->edt_environment_;
  sdf_map_ = edt_environment_->sdf_map_;
  frontier_finder_.reset(new FrontierFinder(edt_environment_, nh)); // 创建前沿发现器
  // uniform_grid_.reset(new UniformGrid(edt_environment_, nh));
  hgrid_.reset(new HGrid(edt_environment_, nh)); // 创建层次化网格管理器
  // view_finder_.reset(new ViewFinder(edt_environment_, nh));

  // 创建探索数据和参数对象
  ed_.reset(new ExplorationData);
  ep_.reset(new ExplorationParam);

  nh.param("exploration/refine_local", ep_->refine_local_, true);
  nh.param("exploration/refined_num", ep_->refined_num_, -1);
  nh.param("exploration/refined_radius", ep_->refined_radius_, -1.0);
  nh.param("exploration/top_view_num", ep_->top_view_num_, -1);
  nh.param("exploration/max_decay", ep_->max_decay_, -1.0);
  nh.param("exploration/tsp_dir", ep_->tsp_dir_, string("null"));
  nh.param("exploration/mtsp_dir", ep_->mtsp_dir_, string("null"));
  nh.param("exploration/relax_time", ep_->relax_time_, 1.0);
  nh.param("exploration/drone_num", ep_->drone_num_, 1);
  nh.param("exploration/drone_id", ep_->drone_id_, 1);
  nh.param("exploration/init_plan_num", ep_->init_plan_num_, 2);

  ed_->swarm_state_.resize(ep_->drone_num_); // 初始化无人机状态数组
  ed_->pair_opt_stamps_.resize(ep_->drone_num_); // 初始化成对优化的时间戳数组
  ed_->pair_opt_res_stamps_.resize(ep_->drone_num_); // 成对优化回复时间戳
  // 设置初始时间戳为 0
  for (int i = 0; i < ep_->drone_num_; ++i) {
    ed_->swarm_state_[i].stamp_ = 0.0;
    ed_->pair_opt_stamps_[i] = 0.0;
    ed_->pair_opt_res_stamps_[i] = 0.0;
  }
  // 初始化群体轨迹数据
  planner_manager_->swarm_traj_data_.init(ep_->drone_id_, ep_->drone_num_);

  nh.param("exploration/vm", ViewNode::vm_, -1.0);
  nh.param("exploration/am", ViewNode::am_, -1.0);
  nh.param("exploration/yd", ViewNode::yd_, -1.0);
  nh.param("exploration/ydd", ViewNode::ydd_, -1.0);
  nh.param("exploration/w_dir", ViewNode::w_dir_, -1.0);

  // 初始化图搜索相关（hgrid等）的节点
  ViewNode::astar_.reset(new Astar);
  ViewNode::astar_->init(nh, edt_environment_);
  ViewNode::map_ = sdf_map_;

  double resolution_ = sdf_map_->getResolution();
  Eigen::Vector3d origin, size;
  sdf_map_->getRegion(origin, size);
  ViewNode::caster_.reset(new RayCaster);
  ViewNode::caster_->setParams(resolution_, origin);

  planner_manager_->path_finder_->lambda_heu_ = 1.0;
  // planner_manager_->path_finder_->max_search_time_ = 0.05;
  planner_manager_->path_finder_->max_search_time_ = 1.0;

  // init astp and cvrp solver
  tsp_client_ =
      nh.serviceClient<lkh_mtsp_solver::SolveMTSP>("/solve_tsp_" + to_string(ep_->drone_id_), true);
  acvrp_client_ = nh.serviceClient<lkh_mtsp_solver::SolveMTSP>(
      "/solve_acvrp_" + to_string(ep_->drone_id_), true);

  // Swarm
  // 初始化无人机群体的状态参数
  for (auto& state : ed_->swarm_state_) {
    state.stamp_ = 0.0;
    state.recent_interact_time_ = 0.0;
    state.recent_attempt_time_ = 0.0;
  }

  // 设置最后分配的网格 ID 和重新分配标志
  ed_->last_grid_ids_ = {};
  ed_->reallocated_ = true;
  ed_->pair_opt_stamp_ = 0.0;
  ed_->wait_response_ = false;
  ed_->plan_num_ = 0;
}




}